#!/usr/bin/python
import smbus
import math
import types
import ctypes
import time

I2C_ADDRES = 0x50
X_ACC_ADDR = 0x34
Y_ACC_ADDR = 0x35
Z_ACC_ADDR = 0x36
X_ANG_VEL_ADDR = 0x37
Y_ANG_VEL_ADDR = 0x38
Z_ANG_VEL_ADDR = 0x39
Q0_QUATERNION = 0x51
Q1_QUATERNION = 0x52
Q2_QUATERNION = 0x53
Q3_QUATERNION = 0x54
ROLL = 0x3d
PITCH = 0x3e
YAW = 0x3f
G = 9.8

class JY901():
    
    def __init__(self):
        self.bus = smbus.SMBus(1)

    def calc_accelerate_value(self):
        x_data = self.bus.read_i2c_block_data(I2C_ADDRES, X_ACC_ADDR, 2)
        y_data = self.bus.read_i2c_block_data(I2C_ADDRES, Y_ACC_ADDR, 2)
        z_data = self.bus.read_i2c_block_data(I2C_ADDRES, Z_ACC_ADDR, 2)
        x_accelerate = ((x_data[1]<<8)|x_data[0])/32768*16
        y_accelerate = ((y_data[1]<<8)|y_data[0])/32768*16
        z_accelerate = ((z_data[1]<<8)|z_data[0])/32768*16
        if x_accelerate >= 16:
            x_accelerate -= 2 * 16
        if y_accelerate >= 16:
            y_accelerate -= 2 * 16
        if z_accelerate >= 16:
            z_accelerate -= 2 * 16       
        return x_accelerate, y_accelerate, z_accelerate

    def calc_angle_vel_value(self):
        x_data = self.bus.read_i2c_block_data(I2C_ADDRES, X_ANG_VEL_ADDR, 2)
        y_data = self.bus.read_i2c_block_data(I2C_ADDRES, Y_ANG_VEL_ADDR, 2)
        z_data = self.bus.read_i2c_block_data(I2C_ADDRES, Z_ANG_VEL_ADDR, 2)
        x_angle_vel = ((x_data[1]<<8)|x_data[0])/32768*2000
        y_angle_vel = ((y_data[1]<<8)|y_data[0])/32768*2000
        z_angle_vel = ((z_data[1]<<8)|z_data[0])/32768*2000
        if x_angle_vel >= 2000:
            x_angle_vel -= 2 * 2000
        if y_angle_vel >= 2000:
            y_angle_vel -= 2 * 2000
        if z_angle_vel >= 2000:
            z_angle_vel -= 2 * 2000  
        return x_angle_vel, y_angle_vel, z_angle_vel

    def calc_quaternion_value(self):
        q_0_data = self.bus.read_i2c_block_data(I2C_ADDRES, Q0_QUATERNION, 2)
        q_1_data = self.bus.read_i2c_block_data(I2C_ADDRES, Q1_QUATERNION, 2)
        q_2_data = self.bus.read_i2c_block_data(I2C_ADDRES, Q2_QUATERNION, 2)
        q_3_data = self.bus.read_i2c_block_data(I2C_ADDRES, Q3_QUATERNION, 2)
        q_0 = ((q_0_data[1]<<8)|q_0_data[0])/32768
        q_1 = ((q_1_data[1]<<8)|q_1_data[0])/32768
        q_2 = ((q_2_data[1]<<8)|q_2_data[0])/32768
        q_3 = ((q_3_data[1]<<8)|q_3_data[0])/32768
        if q_0 >= 1:
            q_0 -= 2 * 1
        if q_1 >= 1:
            q_1 -= 2 * 1
        if q_2 >= 1:
            q_2 -= 2 * 1
        if q_3 >= 1:
            q_3 -= 2 * 1      
        return q_0, q_1, q_2, q_3

    def calc_angle_value(self):
        x_data = self.bus.read_i2c_block_data(I2C_ADDRES, ROLL, 2)
        y_data = self.bus.read_i2c_block_data(I2C_ADDRES, PITCH, 2)
        z_data = self.bus.read_i2c_block_data(I2C_ADDRES, YAW, 2)
        roll = ((x_data[1]<<8)|x_data[0])/32768*180
        pitch = ((y_data[1]<<8)|y_data[0])/32768*180
        yaw = ((z_data[1]<<8)|z_data[0])/32768*180
        if roll >= 180:
            roll -= 360
        if pitch >= 180:
            pitch -= 360
        if yaw >= 2180:
            yaw -= 360  
        roll = roll / 180 * math.pi
        pitch = pitch / 180 * math.pi
        yaw = yaw / 180 * math.pi
        return roll, pitch, yaw